package graph

import "container/heap"

// Dijkstra算法的改进
// Dijkstra算法没有考虑目标点信息， 导致许多搜索都是无效的，在大规模的数据下，导致算法的时间复杂度大幅度提升
// AStar(A*)算法正是基于此的改进的算法
// 可以参考 https://github.com/beefsack/go-astar 的实现
func AStar(from, to Pather) (path []Pather, distance float64, found bool) {
	nm := nodeMap{}
	nq := &priorityQueue{}
	heap.Init(nq)
	fromNode := nm.get(from)
	fromNode.open = true
	heap.Push(nq, fromNode)

	for {
		if nq.Len() <= 0 {
			break
		}

		current := heap.Pop(nq).(*node)
		current.open = false
		current.closed = true
		if current == nm.get(to) {
			p := make([]Pather, 0)
			curr := current
			for curr != nil {
				p = append(p, curr.pather)
				curr = curr.parent
			}
			return p, current.cost, true
		}

		for _, neighbor := range current.pather.PathNeighbors() {
			cost := current.cost + current.pather.PathNeighborsCost(neighbor)
			neighborNode := nm.get(neighbor)

			if cost < neighborNode.cost {
				if neighborNode.open {
					heap.Remove(nq, neighborNode.index)
				}
				neighborNode.open = false
				neighborNode.closed = false
			}
			if !neighborNode.open && !neighborNode.closed {
				neighborNode.cost = cost
				neighborNode.open = true
				//f(n) = g(n) + h(n)
				neighborNode.rank = cost + neighbor.PathEstimatedCost(to)
				neighborNode.parent = current
				heap.Push(nq, neighborNode)
			}
		}
	}

	return nil, 0, false
}

type Pather interface {
	PathNeighbors() []Pather
	PathNeighborsCost(to Pather) float64
	PathEstimatedCost(to Pather) float64
}

type node struct {
	pather Pather
	cost   float64
	rank   float64
	parent *node
	open   bool
	closed bool
	index  int
}

type nodeMap map[Pather]*node

func (nm nodeMap) get(p Pather) *node {
	n, ok := nm[p]
	if !ok {
		n = &node{
			pather: p,
		}
		nm[p] = n
		return n
	}
	return n
}

type priorityQueue []*node

func (p priorityQueue) Len() int {
	return len(p)
}

func (p priorityQueue) Less(i, j int) bool {
	return p[i].rank < p[j].rank
}

func (p priorityQueue) Swap(i, j int) {
	p[i], p[j] = p[j], p[i]
	p[i].index = i
	p[j].index = j
}

func (p *priorityQueue) Push(x interface{}) {
	n := len(*p)
	no := x.(*node)
	no.index = n
	*p = append(*p, no)
}

func (p *priorityQueue) Pop() (v interface{}) {
	old := *p
	n := len(old)
	item := old[n-1]
	old[n-1] = nil  // avoid memory leak
	item.index = -1 // for safety
	*p = old[0 : n-1]
	return item
}
